
/***********************************************************************************************************************
**                                                    Includes                                                        **
***********************************************************************************************************************/
#include <string.h>
#include <stdlib.h>
#include "gd32f30x_misc.h"
#include "Diag.h"
#include "CanTp.h"
#include "Rte.h"


/***********************************************************************************************************************
**                                           Global Variable Definitions                                              **
***********************************************************************************************************************/
__align(16) uint16 MagicFlag __attribute__((at(0x20017FF0)));


/***********************************************************************************************************************
**                                            Private Macro Definitions                                               **
***********************************************************************************************************************/
#define DIAG_ENABLE_INTERRUPT()         __asm ("CPSIE  I")
#define DIAG_DISABLE_INTERRUPT()        __asm ("CPSID  I")

#define DIAG_READ_DID_NUMBER            (50u)
#define DIAG_WRITE_DID_NUMBER           (11u)

/*invalid io control command*/
#define DIAG_IOCTROLCMD_INVALID         (0xFFu)

/***********************************************************************************************************************
**                                            Private Type Definitions                                                **
***********************************************************************************************************************/
/*state for uds communication connection*/
typedef enum
{
    DIAG_STATE_IDLE = 0U,
    DIAG_STATE_PROCESSING,
    DIAG_STATE_WAIT_CONFIRM,
} Diag_StateType;

typedef struct
{
    uint8 reqData[200];
    uint8 reqDataLen;
    uint8 resData[200];
    uint8 resDataLen;
    Diag_StateType state;
} Diag_MsgContextType;

/*internal control*/
typedef enum
{
    DIAG_DEFAULT_SESSION = 1U,
    DIAG_PROGRAMMING_SESSION,
    DIAG_EXTENDED_DIAGNOSTIC_SESSION,
} Diag_SessionType;

typedef struct
{
    boolean checkProgPreConditionFlag;
    Diag_SessionType sessionType;
} Diag_InternalCtrlType;

/*function requests to the application a data value*/
typedef void ( * Diag_ReadDidFncType)(uint8 *p, uint8 len);
typedef void ( * Diag_WriteDidFncType)(uint8 *p, uint8 len);

typedef struct
{
    uint16 did; /*Did Identifier*/
    uint16 len; /*Did length*/
    Diag_ReadDidFncType readDidFnc;
} Diag_ReadDIDCfgType;

typedef struct
{
    uint16 did; /*Did Identifier*/
    uint16 len; /*Did length*/
    Diag_WriteDidFncType writeDidFnc;
} Diag_WriteDIDCfgType;

/***********************************************************************************************************************
**                                           Private Variable Definitions                                             **
***********************************************************************************************************************/
/*diagnostic function module internal control variables*/
static Diag_MsgContextType Diag_MsgContext;
static Diag_InternalCtrlType Diag_InternalCtrl;
/*0x27 service*/
static uint8 Diag_Lev1_Seed[4];
static uint8 Diag_Lev1_Key[4];
static uint8 Diag_Lev1_Cal[4];
static const uint8 Diag_Lev1_Xor[4] = {0xACu, 0x42u, 0x33u, 0x7Au};
/*0x22 service*/
static const Diag_ReadDIDCfgType Diag_ReadDIDCfg[DIAG_READ_DID_NUMBER] =
{
    {0xF187u, 10u, Rte_Diag_Read_DID_F187},
    {0xF188u, 10u, Rte_Diag_Read_DID_F188},
    {0xF18Au, 04u, Rte_Diag_Read_DID_F18A},
    {0xF18Cu, 11u, Rte_Diag_Read_DID_F18C},
    {0xF190u, 17u, Rte_Diag_Read_DID_F190},
    {0xF163u, 10u, Rte_Diag_Read_DID_F163},
    {0xF180u, 10u, Rte_Diag_Read_DID_F180},
    {0xF19Eu, 10u, Rte_Diag_Read_DID_F19E},
    {0xF194u, 11u, Rte_Diag_Read_DID_F194},
    {0x0301u, 01u, Rte_Diag_Read_DID_0301},
    {0x0302u, 02u, Rte_Diag_Read_DID_0302},
    {0x0303u, 02u, Rte_Diag_Read_DID_0303},
    {0x0304u, 02u, Rte_Diag_Read_DID_0304},
    {0x0305u, 02u, Rte_Diag_Read_DID_0305},
    {0x0306u, 02u, Rte_Diag_Read_DID_0306},
    {0x0307u, 02u, Rte_Diag_Read_DID_0307},
    {0x0308u, 02u, Rte_Diag_Read_DID_0308},
    {0x0309u, 01u, Rte_Diag_Read_DID_0309},
    {0x0310u, 01u, Rte_Diag_Read_DID_0310},
    {0x0311u, 01u, Rte_Diag_Read_DID_0311},
    {0x0312u, 01u, Rte_Diag_Read_DID_0312},
    {0x0313u, 01u, Rte_Diag_Read_DID_0313},
    {0x0314u, 01u, Rte_Diag_Read_DID_0314},
    {0x0315u, 01u, Rte_Diag_Read_DID_0315},
    {0x0316u, 01u, Rte_Diag_Read_DID_0316},
    {0x0317u, 02u, Rte_Diag_Read_DID_0317},
    {0x0318u, 01u, Rte_Diag_Read_DID_0318},
    {0x0319u, 01u, Rte_Diag_Read_DID_0319},
    {0x0320u, 01u, Rte_Diag_Read_DID_0320},
    {0x0321u, 02u, Rte_Diag_Read_DID_0321},
    {0x0322u, 02u, Rte_Diag_Read_DID_0322},
    {0x0323u, 02u, Rte_Diag_Read_DID_0323},
    {0x0324u, 02u, Rte_Diag_Read_DID_0324},
    {0x0325u, 02u, Rte_Diag_Read_DID_0325},
    {0x0326u, 02u, Rte_Diag_Read_DID_0326},
    {0x0327u, 02u, Rte_Diag_Read_DID_0327},
    {0x0328u, 02u, Rte_Diag_Read_DID_0328},
    {0x0329u, 02u, Rte_Diag_Read_DID_0329},
    {0x0330u, 01u, Rte_Diag_Read_DID_0330},
    {0x0331u, 04u, Rte_Diag_Read_DID_0331},
    {0x0332u, 07u, Rte_Diag_Read_DID_0332},
    {0x0333u, 01u, Rte_Diag_Read_DID_0333},
    {0x0334u, 01u, Rte_Diag_Read_DID_0334},
    {0x0335u, 01u, Rte_Diag_Read_DID_0335},
    {0x0336u, 02u, Rte_Diag_Read_DID_0336},
    {0x0337u, 03u, Rte_Diag_Read_DID_0337},
    {0x0338u, 01u, Rte_Diag_Read_DID_0338},
    {0x0339u, 01u, Rte_Diag_Read_DID_0339},
    {0x0340u, 02u, Rte_Diag_Read_DID_0340},
    {0x0341u, 01u, Rte_Diag_Read_DID_0341}
};
/*0x2E service*/
static const Diag_WriteDIDCfgType Diag_WriteDIDCfg[DIAG_WRITE_DID_NUMBER] =
{
    {0xF190u, 17u, Rte_Diag_Write_DID_F190},
    {0x0321u, 02u, Rte_Diag_Write_DID_0321},
    {0x0322u, 02u, Rte_Diag_Write_DID_0322},
    {0x0323u, 02u, Rte_Diag_Write_DID_0323},
    {0x0324u, 02u, Rte_Diag_Write_DID_0324},
    {0x0325u, 02u, Rte_Diag_Write_DID_0325},
    {0x0331u, 04u, Rte_Diag_Write_DID_0331},
    {0x0337u, 03u, Rte_Diag_Write_DID_0337},
    {0x0339u, 01u, Rte_Diag_Write_DID_0339},
    {0x0340u, 02u, Rte_Diag_Write_DID_0340},
    {0x0341u, 01u, Rte_Diag_Write_DID_0341}
};
/*0x2F service*/
static uint8 Diag_IOCtrolCmd_3031 = DIAG_IOCTROLCMD_INVALID;
static uint8 Diag_IOCtrolCmd_3032 = DIAG_IOCTROLCMD_INVALID;
static uint8 Diag_IOCtrolCmd_3033 = DIAG_IOCTROLCMD_INVALID;


/***********************************************************************************************************************
**                                           Private Function Declarations                                            **
***********************************************************************************************************************/
static void Diag_ProcessorUDS0x27(void);
static void Diag_ProcessorUDS0x11(void);
static void Diag_ProcessorUDS0x22(void);
static void Diag_ProcessorUDS0x2E(void);
static void Diag_ProcessorUDS0x2F(void);
static void Diag_ProcessorUDS0x14(void);
static void Diag_ProcessorUDS0x10(void);
static void Diag_ProcessorUDS0x31(void);
static void Diag_ProcessorUDS0x85(void);
static void Diag_ProcessorUDS0x28(void);
static void Diag_NotSupportServices(void);
static void Diag_IOControl_Test(uint16 did, uint8 cmd);

/***********************************************************************************************************************
**                                            Global Function Definitions                                             **
***********************************************************************************************************************/
/*This function initializes the diagnostic module*/
void Diag_Init(void)
{
    memset(&Diag_MsgContext, 0, sizeof(Diag_MsgContext));
    Diag_InternalCtrl.checkProgPreConditionFlag = FALSE;
    Diag_InternalCtrl.sessionType = DIAG_DEFAULT_SESSION;
}

void Diag_RxIndication(uint8 length, uint8 const* dataPtr)
{
    /*copy data to diag module buffer.*/
    Diag_MsgContext.reqDataLen = length;
    memcpy(&(Diag_MsgContext.reqData[0]), dataPtr, length);
    if (DIAG_STATE_IDLE == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_PROCESSING;
        switch (Diag_MsgContext.reqData[0])
        {
        case 0x27u:
            Diag_ProcessorUDS0x27();
            break;
        case 0x11u:
            Diag_ProcessorUDS0x11();
            break;
        case 0x22u:
            Diag_ProcessorUDS0x22();
            break;
        case 0x2Eu:
            Diag_ProcessorUDS0x2E();
            break;
        case 0x2Fu:
            Diag_ProcessorUDS0x2F();
            break;
        case 0x14u:
            Diag_ProcessorUDS0x14();
            break;
        case 0x10u:
            Diag_ProcessorUDS0x10();
            break;
        case 0x31u:
            Diag_ProcessorUDS0x31();
            break;
        case 0x85u:
            Diag_ProcessorUDS0x85();
            break;
        case 0x28u:
            Diag_ProcessorUDS0x28();
            break;
        default:
            /*not support service*/
            Diag_NotSupportServices();
            break;
        }
    }
}


void Diag_TxConfirmation(void)
{
    if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        switch (Diag_MsgContext.reqData[0])
        {
        case 0x27u:
            Diag_ProcessorUDS0x27();
            break;
        case 0x11u:
            Diag_ProcessorUDS0x11();
            break;
        case 0x22u:
            Diag_ProcessorUDS0x22();
            break;
        case 0x2Eu:
            Diag_ProcessorUDS0x2E();
            break;
        case 0x2Fu:
            Diag_ProcessorUDS0x2F();
            break;
        case 0x14u:
            Diag_ProcessorUDS0x14();
            break;
        case 0x10u:
            Diag_ProcessorUDS0x10();
            break;
        case 0x31u:
            Diag_ProcessorUDS0x31();
            break;
        case 0x85u:
            Diag_ProcessorUDS0x85();
            break;
        case 0x28u:
            Diag_ProcessorUDS0x28();
            break;
        default:
            /*not support service*/
            Diag_NotSupportServices();
            break;
        }
    }
}


void Diag_MainFunction(void)
{
    /*IO control process*/
    Diag_IOControl_Test(0x3031u, Diag_IOCtrolCmd_3031);
    Diag_IOControl_Test(0x3032u, Diag_IOCtrolCmd_3032);
    Diag_IOControl_Test(0x3033u, Diag_IOCtrolCmd_3033);
}



/***********************************************************************************************************************
**                                           Private Function Definitions                                             **
***********************************************************************************************************************/
static void Diag_ProcessorUDS0x27(void)
{
    uint32 tmpSeed;
    Std_ReturnType ret;
    uint8 i;

    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        /*get seed*/
        if (0x01u == Diag_MsgContext.reqData[1])
        {
            srand(1);
            tmpSeed = rand();
            Diag_Lev1_Seed[0] = (uint8)(tmpSeed >> 24u);
            Diag_Lev1_Seed[1] = (uint8)(tmpSeed >> 16u);
            Diag_Lev1_Seed[2] = (uint8)(tmpSeed >> 8u);
            Diag_Lev1_Seed[3] = (uint8)tmpSeed;

            Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
            Diag_MsgContext.resData[1] = 0x01u;
            Diag_MsgContext.resData[2] = Diag_Lev1_Seed[0];
            Diag_MsgContext.resData[3] = Diag_Lev1_Seed[1];
            Diag_MsgContext.resData[4] = Diag_Lev1_Seed[2];
            Diag_MsgContext.resData[5] = Diag_Lev1_Seed[3];
            Diag_MsgContext.resDataLen = 6u;
        }
        /*compare key*/
        else if (0x02u == Diag_MsgContext.reqData[1])
        {
            for (i = 0; i < 4u; i++)
            {
                Diag_Lev1_Cal[i] = Diag_Lev1_Seed[i] ^ Diag_Lev1_Xor[i];
            }
            Diag_Lev1_Key[0]=((Diag_Lev1_Cal[1] & 0x3F) | (Diag_Lev1_Cal[3] & 0xF1));
            Diag_Lev1_Key[1]=((Diag_Lev1_Cal[0] & 0x0F << 4) | (Diag_Lev1_Cal[1] & 0xF3));
            Diag_Lev1_Key[2]=((Diag_Lev1_Cal[2] & 0xF0 >> 4) | (Diag_Lev1_Cal[3] & 0x3C << 4));
            Diag_Lev1_Key[3]=((Diag_Lev1_Cal[2] & 0x3D) | (Diag_Lev1_Cal[0] & 0x4C >> 4));

            Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
            Diag_MsgContext.resData[1] = 0x02u;
            Diag_MsgContext.resData[2] = Diag_Lev1_Key[0];
            Diag_MsgContext.resData[3] = Diag_Lev1_Key[1];
            Diag_MsgContext.resData[4] = Diag_Lev1_Key[2];
            Diag_MsgContext.resData[5] = Diag_Lev1_Key[3];
            Diag_MsgContext.resDataLen = 6u;
        }
        /*do not support sub-function*/
        else
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x11u;
            Diag_MsgContext.resDataLen = 3u;
        }
        
        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x11(void)
{
    Std_ReturnType ret;
    
    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        if (0x01u == Diag_MsgContext.reqData[1])
        {
            Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[1];
            Diag_MsgContext.resDataLen = 2u;
        }
        /*do not support sub-function*/
        else
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x11u;
            Diag_MsgContext.resDataLen = 3u;
        }
        
        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        /*for debug*/
        if (0x01u == Diag_MsgContext.reqData[1])
        {
            NVIC_SystemReset();
        }
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x22(void)
{
    uint16 did;
    Std_ReturnType ret;
    uint8 i;

    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        did = Diag_MsgContext.reqData[1];
        did = (uint16)(did << 8u) + Diag_MsgContext.reqData[2];
        for (i = 0; i < DIAG_READ_DID_NUMBER; i++)
        {
            if (did == Diag_ReadDIDCfg[i].did)
            {
                Diag_ReadDIDCfg[i].readDidFnc(&(Diag_MsgContext.resData[3]), Diag_ReadDIDCfg[i].len);
                Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
                Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[1];
                Diag_MsgContext.resData[2] = Diag_MsgContext.reqData[2];
                Diag_MsgContext.resDataLen = Diag_ReadDIDCfg[i].len + 3u;
                break;
            }
        }

        /*do not support did*/
        if (DIAG_READ_DID_NUMBER == i)
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x31u;
            Diag_MsgContext.resDataLen = 3u;
        }

        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x2E(void)
{
    uint16 did;
    Std_ReturnType ret;
    uint8 i;

    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        did = Diag_MsgContext.reqData[1];
        did = (uint16)(did << 8u) + Diag_MsgContext.reqData[2];
        for (i = 0; i < DIAG_WRITE_DID_NUMBER; i++)
        {
            if (did == Diag_WriteDIDCfg[i].did)
            {
                Diag_WriteDIDCfg[i].writeDidFnc(&(Diag_MsgContext.reqData[3]), Diag_WriteDIDCfg[i].len);
                Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
                Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[1];
                Diag_MsgContext.resData[2] = Diag_MsgContext.reqData[2];
                Diag_MsgContext.resDataLen = 3u;
                break;
            }
        }

        /*do not support did*/
        if (DIAG_READ_DID_NUMBER == i)
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x31u;
            Diag_MsgContext.resDataLen = 3u;
        }

        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x2F(void)
{
    uint16 did;
    Std_ReturnType ret;

    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        did = Diag_MsgContext.reqData[1];
        did = (uint16)(did << 8u) + Diag_MsgContext.reqData[2];

        /*shortTermAdjustment, returnControlToECU*/
        if ((0x03u == Diag_MsgContext.reqData[3]) || (0x0u == Diag_MsgContext.reqData[3]))
        {
            Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[1];
            Diag_MsgContext.resData[2] = Diag_MsgContext.reqData[2];
            Diag_MsgContext.resData[3] = Diag_MsgContext.reqData[3];
            Diag_MsgContext.resData[4] = Diag_MsgContext.reqData[4];
            Diag_MsgContext.resDataLen = 5u;
            
            if (0x3031u == did)
            {
                if (0x03u == Diag_MsgContext.reqData[3])
                {
                    Diag_IOCtrolCmd_3031 = Diag_MsgContext.reqData[4];
                }
                else
                {
                    /*returnControlToECU, set to invalid cmd*/
                    Diag_IOCtrolCmd_3031 = DIAG_IOCTROLCMD_INVALID;
                }
            }
            else if (0x3032u == did)
            {
                if (0x03u == Diag_MsgContext.reqData[3])
                {
                    Diag_IOCtrolCmd_3032 = Diag_MsgContext.reqData[4];
                }
                else
                {
                    /*returnControlToECU, set to invalid cmd*/
                    Diag_IOCtrolCmd_3032 = DIAG_IOCTROLCMD_INVALID;
                }
            }
            else if (0x3033u == did)
            {
                if (0x03u == Diag_MsgContext.reqData[3])
                {
                    Diag_IOCtrolCmd_3033 = Diag_MsgContext.reqData[4];
                }
                else
                {
                    /*returnControlToECU, set to invalid cmd*/
                    Diag_IOCtrolCmd_3033 = DIAG_IOCTROLCMD_INVALID;
                }
            }
            /*do not support did*/
            else
            {
                Diag_MsgContext.resData[0] = 0x7Fu;
                Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
                Diag_MsgContext.resData[2] = 0x31u;
                Diag_MsgContext.resDataLen = 3u;
            }
        }
        /*do not support sub-function*/
        else
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x11u;
            Diag_MsgContext.resDataLen = 3u;
        }

        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x14(void)
{
    Std_ReturnType ret;
    
    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        if (0xFFu == Diag_MsgContext.reqData[1])
        {
            Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
            Diag_MsgContext.resDataLen = 1u;
            
            //TODO:clear all DTC
        }
        /*do not support sub-function*/
        else
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x11u;
            Diag_MsgContext.resDataLen = 3u;
        }
        
        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x10(void)
{
    Std_ReturnType ret;
    
    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        if ((0x01u == Diag_MsgContext.reqData[1]) 
            || (0x02u == Diag_MsgContext.reqData[1]) 
            || (0x03u == Diag_MsgContext.reqData[1]))
        {
            Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[1];
            Diag_MsgContext.resDataLen = 2u;
        }
        /*do not support sub-function*/
        else
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x11u;
            Diag_MsgContext.resDataLen = 3u;
        }
        
        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        /*enter extended diagnostic session.(enter bootloader code)*/
        if (0x02u == Diag_MsgContext.reqData[1])
        {
            MagicFlag = 0x55AAu;
            NVIC_SystemReset();
        }
        /*enter extended diagnostic session*/
        else if (0x03u == Diag_MsgContext.reqData[1])
        {
            Diag_InternalCtrl.sessionType = DIAG_EXTENDED_DIAGNOSTIC_SESSION;
        }
        /*enter default diagnostic session*/
        else
        {
            Diag_InternalCtrl.sessionType = DIAG_DEFAULT_SESSION;
        }
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x31(void)
{
    uint16 rid;
    Std_ReturnType ret;

    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        /*startRoutine*/
        if (0x01u == Diag_MsgContext.reqData[1])
        {
            rid = Diag_MsgContext.reqData[2];
            rid = (uint16)(rid << 8u) + Diag_MsgContext.reqData[3];
            
            if (0xFF02u == rid)
            {
                if (DIAG_EXTENDED_DIAGNOSTIC_SESSION == Diag_InternalCtrl.sessionType)
                {
                    if (TRUE == Rte_Diag_CheckProgrammingPrecondition())
                    {
                        Diag_InternalCtrl.checkProgPreConditionFlag = TRUE;
                    }
                    else
                    {
                        Diag_InternalCtrl.checkProgPreConditionFlag = FALSE;
                    }
                    
                    /*positive response*/
                    Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;;
                    Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[1];
                    Diag_MsgContext.resData[2] = Diag_MsgContext.reqData[2];
                    Diag_MsgContext.resData[3] = Diag_MsgContext.reqData[3];
                    Diag_MsgContext.resDataLen = 4u;
                }
                /*do not support session level*/
                else
                {
                    Diag_MsgContext.resData[0] = 0x7Fu;
                    Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
                    Diag_MsgContext.resData[2] = 0x31u;
                    Diag_MsgContext.resDataLen = 3u;
                }
            }
            /*do not support routine id*/
            else
            {
                Diag_MsgContext.resData[0] = 0x7Fu;
                Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
                Diag_MsgContext.resData[2] = 0x31u;
                Diag_MsgContext.resDataLen = 3u;
            }
        }
        /*do not support sub-function*/
        else
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x12u;
            Diag_MsgContext.resDataLen = 3u;
        }

        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x85(void)
{
    Std_ReturnType ret;
    uint8 DTCSettingType;
    
    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        DTCSettingType = Diag_MsgContext.reqData[1];
        
        /*stop updating DTC*/
        if (0x02u == DTCSettingType)
        {
            Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[1];
            Diag_MsgContext.resDataLen = 2u;
            
            //TODO:DTCSetting
        }
        /*do not support sub-function*/
        else
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x12u;
            Diag_MsgContext.resDataLen = 3u;
        }
        
        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}

static void Diag_ProcessorUDS0x28(void)
{
    Std_ReturnType ret;
    uint8 controlType;
    uint8 communicationType;
    
    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        controlType = Diag_MsgContext.reqData[1];
        communicationType = Diag_MsgContext.reqData[2];
        
        /*disable the transmission and the reception, application and network management messages*/
        if ((0x03u == controlType) && (0x03u == communicationType))
        {
            Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0] + 0x40u;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[1];
            Diag_MsgContext.resDataLen = 2u;
            
            //TODO:
        }
        /*do not support sub-function*/
        else
        {
            Diag_MsgContext.resData[0] = 0x7Fu;
            Diag_MsgContext.resData[1] = Diag_MsgContext.reqData[0];
            Diag_MsgContext.resData[2] = 0x12u;
            Diag_MsgContext.resDataLen = 3u;
        }
        
        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}


static void Diag_NotSupportServices(void)
{
    Std_ReturnType ret;

    if (DIAG_STATE_PROCESSING == Diag_MsgContext.state)
    {
        Diag_MsgContext.resData[0] = 0x7Fu; /*Negative Response SID*/
        Diag_MsgContext.resData[0] = Diag_MsgContext.reqData[0];
        Diag_MsgContext.resData[1] = 0x11u;
        Diag_MsgContext.resDataLen = 3u;

        /*send response data*/
        ret = CanTp_Transmit(Diag_MsgContext.resDataLen, &(Diag_MsgContext.resData[0]));
        if (E_OK == ret)
        {
            Diag_MsgContext.state = DIAG_STATE_WAIT_CONFIRM;
        }
    }
    else if (DIAG_STATE_WAIT_CONFIRM == Diag_MsgContext.state)
    {
        Diag_MsgContext.state = DIAG_STATE_IDLE;
    }
}


/*0x2F service, io control cmd. 0-Disable, 1-Enable, 2-invalid */
static void Diag_IOControl_Test(uint16 did, uint8 cmd)
{
    uint8 tmpCmd;
    if (DIAG_IOCTROLCMD_INVALID != cmd)
    {
        tmpCmd = cmd >> 4u;
    }
    else
    {
        tmpCmd = 2u;
    }
    Rte_Diag_IOControl_Test(did, tmpCmd);
}

/***********************************************************************************************************************
**                                                  General Notes                                                     **
***********************************************************************************************************************/

